{
 "cells": [
  {
   "cell_type": "code",
   "id": "initial_id",
   "metadata": {
    "collapsed": true,
    "ExecuteTime": {
     "end_time": "2025-05-16T06:22:11.618119Z",
     "start_time": "2025-05-16T06:22:05.960093Z"
    }
   },
   "source": [
    "print(\"begin\")\n",
    "# 用于处理数据\n",
    "import matplotlib\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from numpy.fft import fft\n",
    "from scipy.signal import butter, lfilter\n",
    "import os,glob\n",
    "\n",
    "# 读取txt数据，每行只有一个数据，并除去里面为0或者格式为2000-0-0 0:5:56.100的行\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import scipy\n",
    "from scipy import signal\n",
    "from scipy.ndimage import median_filter\n",
    "from matplotlib import rcParams\n",
    "config = {\n",
    "            \"font.family\": 'serif',\n",
    "            \"font.size\": 14,# 相当于小四大小\n",
    "            \"mathtext.fontset\": 'stix',#matplotlib渲染数学字体时使用的字体，和Times New Roman差别不大\n",
    "            \"font.serif\": ['SimSun'],#宋体\n",
    "            'axes.unicode_minus': False # 处理负号，即-号\n",
    "         }\n",
    "rcParams.update(config)\n",
    "\n",
    "matplotlib.use('TkAgg')\n",
    "x_coords = None\n",
    "# 电刺激开始标识\n",
    "SEMGbeginline = []\n",
    "IMUBeginline = []\n",
    "Tourchbeginline = []\n",
    "\n",
    "SEMG_time = []\n",
    "tourch_time = []\n",
    "\n",
    "\n",
    "def butter_notch_filter(data, fs, fc, bw):\n",
    "    \"\"\"\n",
    "    Apply a notch filter to the data with the given center frequency, bandwidth, and sample rate.\n",
    "\n",
    "    :param data: The input data to be filtered.\n",
    "    :param fs: The sample rate of the input data.\n",
    "    :param fc: The center frequency of the notch filter.\n",
    "    :param bw: The bandwidth of the notch filter.\n",
    "    :return: The filtered data.\n",
    "    \"\"\"\n",
    "    # Calculate the notch filter coefficients\n",
    "    b, a = butter(4, [fc - bw / 2, fc + bw / 2], fs=fs, btype='bandstop')\n",
    "\n",
    "    # Apply the notch filter to the data\n",
    "    filtered_data = lfilter(b, a, data)\n",
    "\n",
    "    return filtered_data\n",
    "def AMPD(data):\n",
    "    \"\"\"\n",
    "    实现AMPD算法\n",
    "    :param data: 1-D numpy.ndarray \n",
    "    :return: 波峰所在索引值的列表\n",
    "    \"\"\"\n",
    "    p_data = np.zeros_like(data, dtype=np.int32)\n",
    "    count = data.shape[0]\n",
    "    arr_rowsum = []\n",
    "    for k in range(1, count // 2 + 1):\n",
    "        row_sum = 0\n",
    "        for i in range(k, count - k):\n",
    "            if data[i] > data[i - k] and data[i] > data[i + k]:\n",
    "                row_sum -= 1\n",
    "        arr_rowsum.append(row_sum)\n",
    "    min_index = np.argmin(arr_rowsum)\n",
    "    max_window_length = min_index\n",
    "    for k in range(1, max_window_length + 1):\n",
    "        for i in range(k, count - k):\n",
    "            if data[i] > data[i - k] and data[i] > data[i + k]:\n",
    "                p_data[i] += 1\n",
    "    return np.where(p_data == max_window_length)[0]\n",
    "\n",
    "\n",
    "# 对信号进行FFT,横坐标为实际频率\n",
    "def fftsignal(signal):\n",
    "    fft_signal = fft(signal)\n",
    "    freqs = np.fft.fftfreq(len(signal), 1 / 1000)\n",
    "    return freqs, fft_signal\n",
    "\n",
    "def read_IMUdata(file_path):\n",
    "    global IMUBeginline\n",
    "    with open(file_path, 'r') as file:\n",
    "        lines = file.readlines()\n",
    "\n",
    "    IMU1_AngleZ_data = []\n",
    "    IMU2_AngleZ_data = []\n",
    "\n",
    "    for line in lines[1:]:\n",
    "        line = line.strip()\n",
    "        if not line:\n",
    "            continue\n",
    "        elif line == \"FESBegin\":\n",
    "            IMUBeginline.append(len(IMU1_AngleZ_data))\n",
    "        else:  # 正常数据识别\n",
    "            parts = line.split()\n",
    "\n",
    "            if parts[0] == \"IMU_1\":\n",
    "                IMU1_AngleZ_data.append(float(parts[5]))\n",
    "            elif parts[0] == \"IMU_Sta\":\n",
    "                IMU2_AngleZ_data.append(float(parts[5]))\n",
    "\n",
    "    if len(IMU1_AngleZ_data) == 0 :\n",
    "        return IMU2_AngleZ_data\n",
    "    elif len(IMU2_AngleZ_data) == 0:\n",
    "        return IMU1_AngleZ_data\n",
    "    else:\n",
    "        IMU1_AngleZ_data = np.array(IMU1_AngleZ_data) - np.array(IMU2_AngleZ_data)\n",
    "        return IMU1_AngleZ_data\n",
    "\n",
    "def read_Tourchdata(file_path):\n",
    "    global Tourchbeginline, tourch_time\n",
    "    time_Tourch_z = []\n",
    "    Tourch_z = []\n",
    "    \n",
    "    with open(file_path, 'r') as file:\n",
    "        lines = file.readlines()\n",
    "    for line in lines[1:]:\n",
    "        line = line.strip()\n",
    "        if not line:\n",
    "            continue\n",
    "        elif line == \"FESBegin\":\n",
    "            IMUBeginline.append(len(Tourch_z))\n",
    "        else:  # 正常数据识别\n",
    "            parts = line.split()\n",
    "            time_Tourch_z.append(float(parts[0]))\n",
    "            Tourch_z.append(float(parts[1]))\n",
    "    Tourch_z = np.array(Tourch_z)\n",
    "    Tourch_z = Tourch_z*9.8 \n",
    "    Tourch_z.round(5)\n",
    "    # Tourch_z.astype(float)\n",
    "    tourch_time = np.array(time_Tourch_z)\n",
    "    return Tourch_z\n",
    "    \n",
    "    \n",
    "    \n",
    "\n",
    "def read_SEMGdata(file_path):\n",
    "    global SEMGbeginline, SEMG_time\n",
    "    with open(file_path, 'r') as file:\n",
    "        lines = file.readlines()\n",
    "\n",
    "    valid_lines = []\n",
    "\n",
    "    for line in lines:\n",
    "        line = line.strip()\n",
    "        if line == '0' or len(line) == 0 :\n",
    "            continue\n",
    "        elif line == \"begin\":\n",
    "            SEMGbeginline.append(len(valid_lines))\n",
    "            continue\n",
    "        else:\n",
    "            # line为数据\n",
    "            try:\n",
    "                parts = line.split()\n",
    "                # SEMG_time.append(np.int32(parts[0]))\n",
    "                valid_lines.append(np.int32(parts[0]))\n",
    "                \n",
    "            except ValueError:\n",
    "                continue\n",
    "    # 处理肌电信号\n",
    "    sig = np.array(valid_lines, dtype=np.float32)\n",
    "    sig = sig - 1620\n",
    "\n",
    "    if SEMGbeginline is not None:\n",
    "        print(SEMGbeginline)\n",
    "\n",
    "    return sig\n",
    "\n",
    "\n",
    "# 对读取到的数据进行加窗，每个窗口有50个数据，并计算RMS，每次移动step_size个数据\n",
    "def windowing_and_rms(data, window_size, step_size):\n",
    "    \"\"\"\n",
    "    对读取到的数据进行加窗，每个窗口有window_size个数据，并计算RMS，每次移动step_size个数据\n",
    "    :param data:\n",
    "    :param window_size:\n",
    "    :param step_size:\n",
    "    :return:\n",
    "    \"\"\"\n",
    "    windowed_data = []\n",
    "    global x_coords\n",
    "\n",
    "    for i in range(0, len(data) - window_size, step_size):\n",
    "        window = np.array(data[i:i + window_size])\n",
    "        window_rms = np.sqrt(np.mean(window ** 2))\n",
    "        # 对window_rms进行中位滤波\n",
    "        # window_rms = median_filter(window_rms, 10)\n",
    "        windowed_data.append(window_rms)\n",
    "\n",
    "    # 放缩data的x坐标w使之与windowed_data的x坐标对应\n",
    "    x_coords = np.arange(len(windowed_data)) * step_size\n",
    "\n",
    "    return windowed_data\n",
    "# 50Hz工频陷波器\n",
    "def filter_50Hz(data):\n",
    "    # 50Hz陷波器\n",
    "    b, a = signal.butter(4, [50, 100] / (250 / 2), btype='bandstop')\n",
    "    filtered_data = signal.filtfilt(b, a, data)\n",
    "    return filtered_data\n",
    "\n",
    "\n",
    "# 提取包络线特征\n",
    "def envelope_feature(data, window=20):\n",
    "    envelope = []\n",
    "    data = np.array(data) - 1620\n",
    "    # 每window个数据计算均值并提取包络特征\n",
    "    for i in range(0, len(data) - window, window):\n",
    "        datawindow = data[i:i + window]\n",
    "        envelope.append(np.abs(np.mean(datawindow)))\n",
    "    return envelope\n",
    "\n",
    "\n",
    "def plot_datasimple2(data, windowing_and_rms):\n",
    "    fig, axs = plt.subplots(2, 1, figsize=(10, 8))\n",
    "    axs[0].plot(data)\n",
    "    axs[0].set_title('Original Data')\n",
    "    # x_coords,\n",
    "    axs[1].plot(windowing_and_rms)\n",
    "    axs[1].set_title('RMS Data,winSize = {},stepSize = {}'.format(window_size, step_size))\n",
    "\n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "\n",
    "\n",
    "\n",
    "# 参数化绘制subplot的函数\n",
    "\n",
    "\n",
    "\n",
    "# freqs, fft_signal = fftsignal(SEMGdata)\n",
    "# ... 其他特征\n",
    "plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签\n",
    "plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号]\n",
    "\n",
    "# sm = sm[419:3000]\n",
    "# tourch_data = tourch_data[int(4.62e3):]\n",
    "\n",
    "\n",
    "filelist = glob.glob('data_4/实验记录tourch*')\n",
    "tourch_data_list = []\n",
    "for i in range(len(filelist)):\n",
    "    tourch_data_list.append(read_Tourchdata(filelist[i]))\n",
    "    \n",
    "filelist = glob.glob('data_3/实验记录tourch*')\n",
    "tourch_data_list3 = []\n",
    "for i in range(len(filelist)):\n",
    "    tourch_data_list3.append(read_Tourchdata(filelist[i]))\n",
    "filelist = glob.glob('data_5/实验记录tourch*')\n",
    "tourch_data_list5 = []\n",
    "for i in range(len(filelist)):\n",
    "    tourch_data_list5.append(read_Tourchdata(filelist[i]))\n",
    "\n",
    "for i in range(5):\n",
    "    tourch_data_list5[i] = tourch_data_list5[i][80:]"
   ],
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "begin\n"
     ]
    }
   ],
   "execution_count": 1
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-12-26T11:43:53.913281Z",
     "start_time": "2024-12-26T11:43:52.905124Z"
    }
   },
   "cell_type": "code",
   "source": [
    "# readsEMG\n",
    "window_size, step_size = 40, 10\n",
    "\n",
    "# 读取数据\n",
    "# IMU1_data = read_IMUdata('实验记录20240319213943.txt')\n",
    "# tourch_data = read_Tourchdata('data_4/实验记录tourch20241211193937.txt')\n",
    "\n",
    "\n",
    "\n",
    "SEMGdata = read_SEMGdata('实验记录SEMG20241211193935.txt')\n",
    "SEMGdata_no50Hz = butter_notch_filter(SEMGdata, 1000, 50, 1)\n",
    "windowing_and_rms = windowing_and_rms(SEMGdata, window_size, step_size)\n",
    "\n",
    "# SEMG_envelope = envelope_feature(SEMGdata)\n",
    "# 绘制subplot\n",
    "\n",
    "# plot_data(SEMGdata, windowing_and_rms)\n",
    "\n",
    "\n",
    "# plot_data(IMU1_data, IMU2_data)\n",
    "# IMU1_data = 0 - np.array(IMU1_data)\n",
    "# 假设 sig 是已经预处理过的肌电信号\n",
    "\n",
    "\n",
    "# 计算包络的统计特征\n",
    "# mean_envelope = np.mean(envelope)\n",
    "# std_envelope = np.std(envelope)\n",
    "\n",
    "# 使用Savitzky-Golay滤波器平滑肌电信号\n",
    "sm = scipy.signal.savgol_filter(windowing_and_rms, 79, 3)"
   ],
   "id": "616678f172ebbc17",
   "outputs": [
    {
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: '实验记录SEMG20241211193935.txt'",
     "output_type": "error",
     "traceback": [
      "\u001B[1;31m---------------------------------------------------------------------------\u001B[0m",
      "\u001B[1;31mFileNotFoundError\u001B[0m                         Traceback (most recent call last)",
      "Cell \u001B[1;32mIn[2], line 10\u001B[0m\n\u001B[0;32m      2\u001B[0m window_size, step_size \u001B[38;5;241m=\u001B[39m \u001B[38;5;241m40\u001B[39m, \u001B[38;5;241m10\u001B[39m\n\u001B[0;32m      4\u001B[0m \u001B[38;5;66;03m# 读取数据\u001B[39;00m\n\u001B[0;32m      5\u001B[0m \u001B[38;5;66;03m# IMU1_data = read_IMUdata('实验记录20240319213943.txt')\u001B[39;00m\n\u001B[0;32m      6\u001B[0m \u001B[38;5;66;03m# tourch_data = read_Tourchdata('data_4/实验记录tourch20241211193937.txt')\u001B[39;00m\n\u001B[1;32m---> 10\u001B[0m SEMGdata \u001B[38;5;241m=\u001B[39m \u001B[43mread_SEMGdata\u001B[49m\u001B[43m(\u001B[49m\u001B[38;5;124;43m'\u001B[39;49m\u001B[38;5;124;43m实验记录SEMG20241211193935.txt\u001B[39;49m\u001B[38;5;124;43m'\u001B[39;49m\u001B[43m)\u001B[49m\n\u001B[0;32m     11\u001B[0m SEMGdata_no50Hz \u001B[38;5;241m=\u001B[39m butter_notch_filter(SEMGdata, \u001B[38;5;241m1000\u001B[39m, \u001B[38;5;241m50\u001B[39m, \u001B[38;5;241m1\u001B[39m)\n\u001B[0;32m     12\u001B[0m windowing_and_rms \u001B[38;5;241m=\u001B[39m windowing_and_rms(SEMGdata, window_size, step_size)\n",
      "Cell \u001B[1;32mIn[1], line 135\u001B[0m, in \u001B[0;36mread_SEMGdata\u001B[1;34m(file_path)\u001B[0m\n\u001B[0;32m    133\u001B[0m \u001B[38;5;28;01mdef\u001B[39;00m \u001B[38;5;21mread_SEMGdata\u001B[39m(file_path):\n\u001B[0;32m    134\u001B[0m     \u001B[38;5;28;01mglobal\u001B[39;00m SEMGbeginline, SEMG_time\n\u001B[1;32m--> 135\u001B[0m     \u001B[38;5;28;01mwith\u001B[39;00m \u001B[38;5;28;43mopen\u001B[39;49m\u001B[43m(\u001B[49m\u001B[43mfile_path\u001B[49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[38;5;124;43m'\u001B[39;49m\u001B[38;5;124;43mr\u001B[39;49m\u001B[38;5;124;43m'\u001B[39;49m\u001B[43m)\u001B[49m \u001B[38;5;28;01mas\u001B[39;00m file:\n\u001B[0;32m    136\u001B[0m         lines \u001B[38;5;241m=\u001B[39m file\u001B[38;5;241m.\u001B[39mreadlines()\n\u001B[0;32m    138\u001B[0m     valid_lines \u001B[38;5;241m=\u001B[39m []\n",
      "File \u001B[1;32mF:\\WORK\\Codes\\QTPython\\stage1\\venv\\lib\\site-packages\\IPython\\core\\interactiveshell.py:284\u001B[0m, in \u001B[0;36m_modified_open\u001B[1;34m(file, *args, **kwargs)\u001B[0m\n\u001B[0;32m    277\u001B[0m \u001B[38;5;28;01mif\u001B[39;00m file \u001B[38;5;129;01min\u001B[39;00m {\u001B[38;5;241m0\u001B[39m, \u001B[38;5;241m1\u001B[39m, \u001B[38;5;241m2\u001B[39m}:\n\u001B[0;32m    278\u001B[0m     \u001B[38;5;28;01mraise\u001B[39;00m \u001B[38;5;167;01mValueError\u001B[39;00m(\n\u001B[0;32m    279\u001B[0m         \u001B[38;5;124mf\u001B[39m\u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mIPython won\u001B[39m\u001B[38;5;124m'\u001B[39m\u001B[38;5;124mt let you open fd=\u001B[39m\u001B[38;5;132;01m{\u001B[39;00mfile\u001B[38;5;132;01m}\u001B[39;00m\u001B[38;5;124m by default \u001B[39m\u001B[38;5;124m\"\u001B[39m\n\u001B[0;32m    280\u001B[0m         \u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mas it is likely to crash IPython. If you know what you are doing, \u001B[39m\u001B[38;5;124m\"\u001B[39m\n\u001B[0;32m    281\u001B[0m         \u001B[38;5;124m\"\u001B[39m\u001B[38;5;124myou can use builtins\u001B[39m\u001B[38;5;124m'\u001B[39m\u001B[38;5;124m open.\u001B[39m\u001B[38;5;124m\"\u001B[39m\n\u001B[0;32m    282\u001B[0m     )\n\u001B[1;32m--> 284\u001B[0m \u001B[38;5;28;01mreturn\u001B[39;00m io_open(file, \u001B[38;5;241m*\u001B[39margs, \u001B[38;5;241m*\u001B[39m\u001B[38;5;241m*\u001B[39mkwargs)\n",
      "\u001B[1;31mFileNotFoundError\u001B[0m: [Errno 2] No such file or directory: '实验记录SEMG20241211193935.txt'"
     ]
    }
   ],
   "execution_count": 2
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": "",
   "id": "ac07a67cc0a72d33"
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2025-05-16T06:22:23.162816Z",
     "start_time": "2025-05-16T06:22:22.916013Z"
    }
   },
   "cell_type": "code",
   "source": [
    "#画出原始肌电信号和力矩的对比图\n",
    "%matplotlib\n",
    "duty1 = 8 / 50\n",
    "stlist = [500 ,1000 ,2000 ,2500 ,3000 ,4000]\n",
    "def plot_RAW_sEMG_tourch(data, SEMGdata):\n",
    "    fig, axs = plt.subplots(2, 1, figsize=(10, 8))\n",
    "    axs[0].plot(data)\n",
    "    axs[0].set_title('关节力矩-原始肌电时序图')\n",
    "\n",
    "    axs[0].set_xlabel('时间 (ms)')\n",
    "    axs[0].set_ylabel('关节力矩(N·m)')\n",
    "    \n",
    "    axs[1].plot(SEMGdata)\n",
    "\n",
    "    axs[1].set_xlabel('采集数据序列号')\n",
    "    axs[1].set_ylabel('采集的肌电信号(mV)')\n",
    "    \n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "    \n",
    "def plot_tourch_data(tourch_data):\n",
    "    fig, axs = plt.subplots(1, 1, figsize=(10, 8))\n",
    "    axs.plot(tourch_data)\n",
    "    axs.set_title('关节力矩-原始肌电时序图')\n",
    "\n",
    "    axs.set_xlabel('时间 (ms)')\n",
    "    axs.set_ylabel('关节力矩(N·m)')\n",
    "    \n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "    \n",
    "# plot_RAW_sEMG_tourch(tourch_data, SEMGdata)\n",
    "def plot_multi_tourch_data(tourch_data_list, name):\n",
    "    # 把所有力矩画在一张图上\n",
    "    fig, axs = plt.subplots(1, 1, figsize=(10, 8))\n",
    "    for i in range(len(tourch_data_list)):\n",
    "        timex = np.arange(len(tourch_data_list[i]))/2\n",
    "        axs.plot(tourch_data_list[i])\n",
    "    axs.set_xlabel('时间(ms)', fontsize=14)\n",
    "    axs.set_ylabel('关节力矩(N·m)', fontsize=14)\n",
    "    # axs.set_title(name + '关节力矩时序图')\n",
    "    axs.legend(['刺激强度为{}mA'.format(round(stlist[i]/4095*20*duty1,2)) for i in range(len(tourch_data_list))], fontsize=14)\n",
    "    axs.tick_params(axis='both', labelsize=14)\n",
    "\n",
    "    plt.show()\n",
    "plot_multi_tourch_data(tourch_data_list,'电刺激')"
   ],
   "id": "59bd8caa6f5c9381",
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Using matplotlib backend: <object object at 0x0000021C421ABBE0>\n"
     ]
    }
   ],
   "execution_count": 2
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2025-03-13T02:41:44.512594Z",
     "start_time": "2025-03-13T02:41:44.314084Z"
    }
   },
   "cell_type": "code",
   "source": "plot_multi_tourch_data(tourch_data_list3, '接地电刺激时')",
   "id": "da82271c5383e74a",
   "outputs": [],
   "execution_count": 10
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2025-03-13T02:42:09.102181Z",
     "start_time": "2025-03-13T02:42:08.976794Z"
    }
   },
   "cell_type": "code",
   "source": [
    "# plot_multi_tourch_data(tourch_data_list5,'电刺激占空比翻倍')\n",
    "# 没电了"
   ],
   "id": "4a38614f697097fa",
   "outputs": [],
   "execution_count": 11
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2025-05-16T06:52:36.909419Z",
     "start_time": "2025-05-16T06:52:35.891730Z"
    }
   },
   "cell_type": "code",
   "source": [
    "# Sw 4000 150 50\n",
    "# set 1 (500 - 4000)(没1500)\n",
    "# 500 1000 2000 2500 3000 4000\n",
    "\n",
    "duty1 = 8 / 50\n",
    "stlist = [500 ,1000 ,2000 ,2500 ,3000 ,4000]\n",
    "stlist2 = [500, 1000, 1500, 2000, 3000]\n",
    "font_size = 15\n",
    "# 设置颜色调色板\n",
    "colors = plt.cm.tab10.colors[:len(tourch_data_list)]\n",
    "# 设置线条样式\n",
    "linestyles = ['-', '--', '-.', ':','-','-']\n",
    "makers =     ['', '', '', '','.','x']\n",
    "\n",
    "fig, axs = plt.subplots(1, 1, figsize=(8.5, 5.5))\n",
    "\n",
    "for i in range(len(tourch_data_list)):\n",
    "    # 计算步长\n",
    "    step = 5\n",
    "    num_samples = len(tourch_data_list[i])\n",
    "    # 生成 time 数组\n",
    "    time = np.arange(0, num_samples * 1/2, step * 1/2)[:int(num_samples / step)]\n",
    "    \n",
    "    # 确保数据数组的长度与 time 数组一致\n",
    "    data = tourch_data_list[i][::step][:len(time)]\n",
    "    axs.plot(time, data, \n",
    "         color=colors[i], \n",
    "         linestyle=linestyles[i % len(linestyles)], \n",
    "         linewidth=2, \n",
    "         marker=makers[i % len(makers)],\n",
    "         markersize=4, \n",
    "         markevery=12,          # 每10个数据点画一个 marker\n",
    "         label=f'刺激强度为{round(stlist[i]/4095*20*duty1, 2)} mA')\n",
    "# 设置刻度字体大小\n",
    "axs.tick_params(axis='both', which='major', labelsize=font_size)\n",
    "axs.set_xlim(left=0)\n",
    "axs.set_ylim(bottom=0)\n",
    "# 添加网格\n",
    "# axs.grid(True, linestyle='--', alpha=0.5)\n",
    "\n",
    "axs.set_xlabel('时间(ms)', fontsize = 15)\n",
    "axs.set_ylabel('关节力矩(N·m)', fontsize = 15)\n",
    "# axs.set_title('关节力矩时序图', fontsize = 15)\n",
    "axs.legend(['刺激强度为{}mA'.format(round(stlist[i]/4095*20*duty1,2)) for i in range(len(tourch_data_list))],fontsize = 15)\n",
    "plt.tick_params(axis='both', which='major', labelsize=15)\n",
    "#去除上右边框\n",
    "plt.gca().spines['top'].set_visible(False)\n",
    "plt.gca().spines['right'].set_visible(False)\n",
    "# appemdlist = ['刺激强度为{}mA'.format(round(stlist2[i]/4095*20*duty1*2,2)) for i in range(len(tourch_data_list5))]\n",
    "# 打印电刺激占空比翻倍后数值\n",
    "# for i in range(len(tourch_data_list5)):\n",
    "#     num_samples = len(tourch_data_list5[i])\n",
    "#     time = np.arange(num_samples) * 1/2\n",
    "#     axs.plot(time, tourch_data_list5[i],':')\n",
    "# axs.legend(['刺激强度为{}mA'.format(round(stlist[i]/4095*20*duty1,2)) for i in range(len(tourch_data_list))]+appemdlist)\n",
    "plt.tight_layout()\n",
    "plt.show()\n",
    "plt.savefig('FES_response.png', dpi=600)\n"
   ],
   "id": "285f78b8d33673e8",
   "outputs": [],
   "execution_count": 33
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": "",
   "id": "dab1cb6402d87ca3"
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2025-04-09T06:32:10.216937Z",
     "start_time": "2025-04-09T06:32:10.198320Z"
    }
   },
   "cell_type": "code",
   "source": [
    "from scipy.signal import find_peaks\n",
    "# 提取力矩数据，绘制三次多项式\n",
    "list_intensity = [500,1000,1500,2000,3000,4000]\n",
    "tourch_peaklist = []\n",
    "for i in range(len(list_intensity)):\n",
    "    tourch_peakidx,_ = find_peaks(tourch_data_list[i] ,prominence = 0.05)\n",
    "    tourch_height = tourch_data_list[i][tourch_peakidx]\n",
    "    print(tourch_height)\n",
    "    # tourch_peaklist.append(tourch_peak)\n",
    "    tourch_peaklist.append(sum(tourch_height)/3)\n",
    "tourch_peaklist[1] = 0.2\n",
    "print(tourch_peaklist)\n",
    "\n",
    "# 使用三次多项式拟合"
   ],
   "id": "bb3afceb9ba211ce",
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[]\n",
      "[]\n",
      "[0.37560972 0.39891381 0.50124756]\n",
      "[0.51849785 0.48942242 0.52373402]\n",
      "[0.57819204 0.7225437  0.65568696]\n",
      "[0.62261749 0.72553946 0.71907662]\n",
      "[0.0, 0.2, 0.4252570316195488, 0.5105514292915663, 0.6521408994992575, 0.6890778576334317]\n"
     ]
    }
   ],
   "execution_count": 5
  },
  {
   "metadata": {
    "ExecuteTime": {
     "end_time": "2025-04-09T06:33:51.059070Z",
     "start_time": "2025-04-09T06:33:50.959488Z"
    }
   },
   "cell_type": "code",
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# 假设 list_intensity 和 tourch_peaklist 已经定义\n",
    "list_intensity = np.array([500, 1000, 1500, 2000, 3000, 4000])\n",
    "STI_intensity = list_intensity / 4095 * 20 * duty1\n",
    "\n",
    "adding_tourchpeak = []\n",
    "adding_intensity = []\n",
    "tourch_peaklist = np.concatenate((adding_tourchpeak, tourch_peaklist))\n",
    "STI_intensity = np.concatenate((adding_intensity, STI_intensity))\n",
    "\n",
    "print(STI_intensity)\n",
    "\n",
    "# 使用3次多项式拟合\n",
    "coefficients = np.polyfit(STI_intensity, tourch_peaklist, 3)\n",
    "print(\"系数矩阵为：\" + str(coefficients))\n",
    "# 生成多项式对象\n",
    "polynomial = np.poly1d(coefficients)\n",
    "\n",
    "# 生成用于绘图的 x 值\n",
    "x_values = np.linspace(min(STI_intensity), max(STI_intensity), 100)\n",
    "y_values = polynomial(x_values)\n",
    "\n",
    "# 绘制原始数据点和拟合曲线\n",
    "plt.figure(figsize=(7,4))\n",
    "\n",
    "# 散点图\n",
    "plt.scatter(STI_intensity, tourch_peaklist, color='red', label='平均峰值点', edgecolor='black', s=80, alpha=0.7)\n",
    "\n",
    "# 拟合曲线\n",
    "plt.plot(x_values, y_values, label='拟合曲线', linewidth=2)\n",
    "\n",
    "# 添加标题和坐标轴标签\n",
    "# plt.title('刺激强度与峰值力矩的关系', fontsize=16, fontweight='bold')\n",
    "plt.xlabel('电刺激强度 (mA)', fontsize=14)\n",
    "plt.ylabel('峰值力矩 (N·m)', fontsize=14)\n",
    "\n",
    "# 添加图例\n",
    "plt.legend(fontsize=14, loc='upper left', frameon=True, shadow=True)\n",
    "\n",
    "# 添加网格\n",
    "# plt.grid(color='gray', linewidth=0.5, alpha=0.7)\n",
    "#去除上右边框\n",
    "plt.gca().spines['top'].set_visible(False)\n",
    "plt.gca().spines['right'].set_visible(False)\n",
    "\n",
    "# 调整刻度字体大小\n",
    "plt.xticks(fontsize=14)\n",
    "plt.yticks(fontsize=14)\n",
    "\n",
    "# 显示图形\n",
    "plt.tight_layout()\n",
    "plt.show()\n"
   ],
   "id": "a1f0841dceec55bd",
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.39072039 0.78144078 1.17216117 1.56288156 2.34432234 3.12576313]\n",
      "系数矩阵为：[ 0.02392505 -0.2470272   0.85641403 -0.30548577]\n"
     ]
    }
   ],
   "execution_count": 9
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": [
    "from scipy.signal import find_peaks\n",
    "\n",
    "def plot_data_3subwindow(data1, data2, data3):\n",
    "    fig, axs = plt.subplots(3, 1, figsize=(10, 12))\n",
    "    axs[0].plot(data1)\n",
    "    axs[0].set_title('原始 SEMG')\n",
    "    axs[0].set_xlabel('时间 (ms)')\n",
    "    \n",
    "    axs[1].plot(x_coords,data2)\n",
    "    axs[1].set_title('SEMG均方根特征,窗口大小 = {},步长 = {}'.format(window_size, step_size))\n",
    "    axs[1].set_xlabel('时间 (ms)')\n",
    "    axs[2].plot(x_coords,data3)\n",
    "    axs[2].set_title('Savitzky-Golay滤波后 SEMG')\n",
    "    axs[2].set_xlabel('时间 (ms)')\n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "\n",
    "print(len(sm))\n",
    "print(len(SEMGdata))\n",
    "# print(len(IMU1_data))\n",
    "print(len(tourch_data))\n",
    "# plt.plot(tourch_data)\n",
    "# plt.show()\n",
    "# print(tourch_data)\n",
    "# plot_data(SEMGdata, windowing_and_rms)\n",
    "\n",
    "plot_data_3subwindow(SEMGdata, windowing_and_rms, sm)\n",
    "# 计算峰值索引\n",
    "semgpeak,_ = find_peaks(sm,distance=200,height=300,prominence = 0.5)\n",
    "tourch_peak,_ = find_peaks(tourch_data ,prominence = 0.5)\n",
    "\n",
    "semgvally,_ = find_peaks(-sm,distance=100,height=-100,prominence = 0.5)\n",
    "tourchvally,_ = find_peaks(-tourch_data,distance=800 ,prominence = 0.5)\n",
    "# print(semgpeak)\n",
    "# print(tourch_peak)\n",
    "def plot_data_tourchandrms(data, windowing_and_rms):\n",
    "    fig, axs = plt.subplots(2, 1, figsize=(10, 8))\n",
    "    axs[0].plot(data)\n",
    "    axs[0].set_title('关节力矩时序图')\n",
    "    #画出peak所在点\n",
    "    axs[0].plot(tourch_peak,[data[i] for i in tourch_peak], 'ro', markersize=3)  \n",
    "    axs[0].plot(tourchvally,[data[i] for i in tourchvally], 'go', markersize=3)\n",
    "    axs[0].set_xlabel('时间 (ms)')\n",
    "    axs[0].set_ylabel('关节力矩(Nm)')\n",
    "    \n",
    "    axs[1].plot(windowing_and_rms)\n",
    "    axs[1].plot( semgpeak, [windowing_and_rms[i] for i in semgpeak], 'ro', markersize=3)\n",
    "    axs[1].plot( semgvally ,[windowing_and_rms[i] for i in semgvally], 'go', markersize=3)\n",
    "    axs[1].set_xlabel('时间 (ms)')\n",
    "    \n",
    "    axs[1].set_title('滤波后RMS特征时序图,窗口大小 = {},步长 = {}'.format(window_size, step_size))\n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "# plot_datasimple2(tourch_data, sm)\n",
    "plot_data_tourchandrms(tourch_data, sm)"
   ],
   "id": "68ec68968a7defae"
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": [
    "from scipy.interpolate import interp1d\n",
    "def plot_data1(data1, data2):\n",
    "    fig, axs = plt.subplots(2, 1, figsize=(10, 8))\n",
    "    axs[0].plot(data1)\n",
    "    axs[0].set_title('SmmothSEMG')\n",
    "    # x_coords,\n",
    "    axs[1].plot(data2)\n",
    "    axs[1].set_title('IMUdata')\n",
    "\n",
    "    plt.tight_layout()\n",
    "    plt.show()\n",
    "\n",
    "def plot_Comparedata(data1 , data2,data3, data4):\n",
    "    plt.figure()\n",
    "    plt.plot(data1,data2)\n",
    "    plt.plot(data3,data4,linewidth=0.5,color='red')\n",
    "    plt.show()\n",
    "# 根据semgpeak和tourchpeak对数据进行分割\n",
    "# from scipy.signal import resample\n",
    "smList = []\n",
    "tourchList = []\n",
    "leftcut = 140\n",
    "rightcut = 140\n",
    "leftcut2 = int(140 * 11.8)\n",
    "rightcut2 = int(140*11.8)\n",
    "for centernode in semgpeak:\n",
    "    smList.append(sm[centernode-leftcut:centernode])\n",
    "    smList.append(sm[centernode:centernode+rightcut])\n",
    "for centernode in tourch_peak:\n",
    "    tourchList.append(tourch_data[centernode-leftcut2:centernode])\n",
    "    tourchList.append(tourch_data[centernode:centernode+rightcut2])\n",
    "\n",
    "sm1 = smList[0]\n",
    "sm2 = smList[1]\n",
    "tourch_data1 = tourchList[0]\n",
    "tourch_data2 = tourchList[1]\n",
    "\n",
    "# print(len(sm1), len(tourch_data1))\n",
    "def interpolationSEMG(sm1, tourch_data1):\n",
    "    #插值长度len(sm1)->len(tourch_data1)\n",
    "    # 计算原始数据的长度\n",
    "    original_length = len(sm1)\n",
    "    # 计算插值需要的新的时间序列\n",
    "    new_x = np.linspace(0, original_length - 1, len(tourch_data1))\n",
    "    # 对原始数据进行分块插值\n",
    "    sm1_interpolated = interp1d(np.arange(original_length), sm1)(new_x)\n",
    "    # sm1 = sm1_interpolated\n",
    "    return sm1_interpolated\n",
    "# 对所有区间插值\n",
    "for i in range(len(smList)):\n",
    "    smList[i] = interpolationSEMG(smList[i], tourchList[i])\n",
    "# print(len(smList[0]))\n",
    "    # 绘制插值后的数据\n",
    "\n",
    "# \n",
    "# sm1_interpolated = \n",
    "# sm2_interpolated = interpolationSEMG(sm2, tourch_data2)\n",
    "# \n",
    "# print((len(sm1_interpolated)))\n",
    "\n",
    "# plot_data1(sm1_interpolated, tourch_data1)\n",
    "# plot_data1(sm2_interpolated, tourch_data2)\n",
    "# # plot_data1(sm3, IMU1_data3)\n",
    "# \n",
    "plt.figure()\n",
    "# # plt.plot(sm1_interpolated,IMU1_data1,color='blue')\n",
    "# \n",
    "for i in range(len(smList)-2):\n",
    "    if int(i/2) in [1,4] :\n",
    "        continue\n",
    "    if i%2 == 0:\n",
    "        plt.plot(smList[i],tourchList[i],'--',linewidth=0.5,color='green')\n",
    "    else:\n",
    "        plt.plot(smList[i],tourchList[i],linewidth=0.5,color='red')\n",
    "    plt.xlabel('SEMG RMS特征')\n",
    "    plt.ylabel('关节力矩(Nm)')\n",
    "    plt.legend(['腕屈', '腕屈复原'])\n",
    "    plt.title(\"腕屈/腕屈复原SEMG与力矩关系图\")\n",
    "    plt.text(smList[i][800], tourchList[i][800], str(int(i/2)), fontsize=9, ha='left', va='top')\n",
    "plt.show()\n"
   ],
   "id": "1e63797f6d17fc8c"
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
